#include "youbot_relay/move_youbot_arm_client.h"


using namespace move_youbot_arm;

// ======================================================== //


MoveYoubotArmClient::MoveYoubotArmClient(int argc, char** argv)
{
  ros::init(argc, argv, CLIENT_NODE_ARM);
  client = new Client(CLIENT_ACTION_ARM, true);
  client->waitForServer( ros::Duration(10,0) );
}

// ======================================================== //

void MoveYoubotArmClient::setGoal( youbot_relay::MoveYoubotArmGoal g )
{
  goal = g;
}

// ======================================================== //

void MoveYoubotArmClient::setGoal(float joint1_degree, float joint2_degree, float joint3_degree, float joint4_degree, float joint5_degree, float gripper_meters )
{
  goal.arm_joint[0] = joint1_degree*(M_PI/180);//all angles are given in degree -> converting into radians
  goal.arm_joint[1] = joint2_degree*(M_PI/180);
  goal.arm_joint[2] = joint3_degree*(M_PI/180);
  goal.arm_joint[3] = joint4_degree*(M_PI/180);
  goal.arm_joint[4] = joint5_degree*(M_PI/180);

  goal.gripper_position = gripper_meters/2;//the client asks for the full distance of gripper's jaws, but internally each jaw will be moved by half distance
}

// ======================================================== //

bool MoveYoubotArmClient::sendGoal()
{
  if( /*goal == NULL ||*/ !client->isServerConnected() ) return false;
  client->sendGoal(goal);
  return true;
}

// ======================================================== //

bool MoveYoubotArmClient::stop()
{
  if( !client->isServerConnected() ) return false;
  client->cancelAllGoals();
  return true;
}

// ======================================================== //

actionlib::SimpleClientGoalState MoveYoubotArmClient::sendGoalForResult( ros::Duration timeout )
{
  if( /*goal == NULL ||*/ !client->isServerConnected() ) return actionlib::SimpleClientGoalState::ABORTED;
  return client->sendGoalAndWait(goal, timeout);
}

// ======================================================== //

//this method is blocking until goal is reached or aborted
bool MoveYoubotArmClient::moveSynchronous( float joint1_degree, float joint2_degree, float joint3_degree, float joint4_degree, float joint5_degree, float gripper_meters )
{
  setGoal(joint1_degree,joint2_degree,joint3_degree,joint4_degree,joint5_degree,gripper_meters);
  if( client->sendGoalAndWait(goal,ros::Duration(0,0))== actionlib::SimpleClientGoalState::SUCCEEDED ) return true;
  return false;
}

// ======================================================== //

//this method waits for timeout
bool MoveYoubotArmClient::move( float joint1_degree, float joint2_degree, float joint3_degree, float joint4_degree, float joint5_degree, float gripper_meters, ros::Duration timeout)
{
  setGoal(joint1_degree,joint2_degree,joint3_degree,joint4_degree,joint5_degree,gripper_meters);

  return move( timeout );
}

// ======================================================== //

bool MoveYoubotArmClient::move( youbot_relay::MoveYoubotArmGoal g, ros::Duration timeout )
{
  setGoal(g);

  return move(timeout);
}

// ======================================================== //

bool MoveYoubotArmClient::move( ros::Duration timeout )
{
  if( timeout == ros::Duration(0, 0) )//infinite timeout -> do not wait!
  {
    //!TODO: set arm's position immediately

    return sendGoal();//send goal
  }
  else//use timeout
  {
    if( sendGoalForResult(timeout) == actionlib::SimpleClientGoalState::SUCCEEDED )
    {
      //!TODO: goal reached -> set arm's position
      return true;
    }
    else return false;
  }
}

// ======================================================== //

bool MoveYoubotArmClient::getCurrentPosition( float *joint1, float *joint2, float *joint3, float *joint4, float *joint5, float *gripper  )
{
  if( joint1==NULL || joint2 == NULL || joint3==NULL || joint4==NULL || joint5==NULL || gripper==NULL ) return false;//invalid pointers

  //!TODO: Implement tracking of arm's position, see above

  *joint1 = 0;
  *joint2 = 0;
  *joint3 = 0;
  *joint4 = 0;
  *joint5 = 0;
  *gripper = 0;

  return false;
}

// ======================================================== //

actionlib::SimpleClientGoalState MoveYoubotArmClient::getState()
{
  if( !client->isServerConnected() ) return actionlib::SimpleClientGoalState::ABORTED;

  //!TODO: getting state while no goal is active leads to an error :( catch error!
  else return client->getState();
}

// +#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+ //

